#include "formation_avoidance/formation.h"

#define AGENT_ID 0

using namespace std;
using namespace Eigen;

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "avoidacne");
    ros::NodeHandle nh("~");

    const int LOOPRATE = 30;
    ros::Rate loop_rate(LOOPRATE);  
    ROS_INFO("agent %d node start-----",AGENT_ID);

    Eigen::Vector3f start_p(2.8,0,1.4);
    Eigen::Vector3f end_p(7.0,0.0,1.8);

    Formation_Agent formation_agent(AGENT_ID);
    formation_agent.init(nh);

    ros::AsyncSpinner spinner(3); // Use 3 threads
    spinner.start();
    ros::waitForShutdown();

    // ros::spin();
	return 0;

}
